#!  /usr/bin/env python
# coding=utf-8

import rospy
from ti5arm.msg import ti5_targetPos
import json
import socket

# 从dir.txt获取关节角度
def getJdeg(dir):
    

    with open(dir, mode='r') as Jfile:
        Jdata = Jfile.readlines()
        groups_jdeg = (len(Jdata)-1)/3
        Jdeg = [0,0,0,0,0,0]*groups_jdeg
        print("groups of Jdeg:",(len(Jdata)-1)/3)
        if(Jdata[0]=="hand_eye_Jdeg\r\n"):
            print("open hand_eye_Jdeg!")
            for i in range(0,groups_jdeg):
                Jdeg[0+(i*6)] = float(Jdata[2+(i*3)].split(',')[0])
                Jdeg[1+(i*6)] = float(Jdata[2+(i*3)].split(',')[1])
                Jdeg[2+(i*6)] = float(Jdata[2+(i*3)].split(',')[2])
                Jdeg[3+(i*6)] = float(Jdata[2+(i*3)].split(',')[3])
                Jdeg[4+(i*6)] = float(Jdata[2+(i*3)].split(',')[4])
                Jdeg[5+(i*6)] = float(Jdata[2+(i*3)].split(',')[5])
                pass
        else:
            
            print("first line are not hand_eye_Jdeg ",Jdata[0])
            pass
        pass# with

    return Jdeg
    pass


def sockClient():
    sockC = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
    sockC.connect("10.206.158.216",38899)

#simpleTest
def simpleTest(pub):
    jointTarget = ti5_targetPos()

    jointTarget.numOfCmd = 7
    jointTarget.jointIDList = [1, 2, 3 ,4 ,5 ,6, 7]

#jointxRadList is rad
    jointTarget.joint1RadList = [0,     0]
    jointTarget.joint2RadList = [0,     0.3413]
    jointTarget.joint3RadList = [0,     -1.4525]
    jointTarget.joint4RadList = [0,     0]
    jointTarget.joint5RadList = [0,     -0.4596]
    jointTarget.joint6RadList = [0,     0]
    jointTarget.joint7RadList = [0,     -600000]
    jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]


    jointTarget2 = ti5_targetPos()

    jointTarget2.numOfCmd = 7
    jointTarget2.jointIDList = [1, 2, 3 ,4 ,5 ,6, 7]

    jointTarget2.joint1RadList = [0,     0]
    jointTarget2.joint2RadList = [0.3413,     0]
    jointTarget2.joint3RadList = [-1.4525,     0]
    jointTarget2.joint4RadList = [0,     0]
    jointTarget2.joint5RadList = [-0.4596,     0]
    jointTarget2.joint6RadList = [0,     0]
    jointTarget2.joint7RadList = [-600000,0]
    jointTarget2.cmdList = [30,         30,      30,       30,       30,     30,   30]
    print("*********+++++start+++*********")
    rate = rospy.Rate(1)
    tt=0
    while 1:
        tt=tt+1
        print("*******loop:",tt)
        pub.publish(jointTarget)
        print("发送完成1")

        rate.sleep()
        rate.sleep()
        rate.sleep()
        rate.sleep()

        
        
        pub.publish(jointTarget2)
        print("发送完成2")
        
        rate.sleep()
        rate.sleep()
        rate.sleep()
        rate.sleep()
        rate.sleep()
        rate.sleep()

# 调用此函数只需传入角度值
def do_hand_eyeJList(jointTarget,cJ,nJ):
    #jointxRadList is rad
    jointTarget.joint1RadList = [cJ[0]*3.14/180,     nJ[0]*3.14/180]
    jointTarget.joint2RadList = [cJ[1]*3.14/180,     nJ[1]*3.14/180]
    jointTarget.joint3RadList = [cJ[2]*3.14/180,     nJ[2]*3.14/180]
    jointTarget.joint4RadList = [cJ[3]*3.14/180,     nJ[3]*3.14/180]
    jointTarget.joint5RadList = [cJ[4]*3.14/180,     nJ[4]*3.14/180]
    jointTarget.joint6RadList = [cJ[5]*3.14/180,     nJ[5]*3.14/180]
    return jointTarget

# 手眼标定点位1
def hand_eye(pub):
    # 需要延时更长时间 令pause_more=1
    pause_more = 0
    jointTarget = ti5_targetPos()

    jointTarget.numOfCmd = 7
    jointTarget.jointIDList = [1, 2, 3 ,4 ,5 ,6, 7]

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start+++*********")
    pp =0

    #from pose 1 to pose 2
    if(0):
        #jointxRadList is rad
        cJ = [0,0,0,0,0,0]
        nJ = [0,0,0,0,158.6994,0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()
        
    #from pose 2 to pose 3
    if(0):
        #jointxRadList is rad
        cJ = [0,0,0,0,158.6994,0]
        nJ = [ 0 ,     0 ,-145.5463  ,       0 , 158.6994  ,       0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 3 to pose 4
    if(0):
        #jointxRadList is rad
        cJ = [0 ,     0 ,-145.5463  ,       0 , 158.6994  ,       0]
        nJ = [90.0000, -13.1531, -145.5463  ,       0,  158.6994  ,     0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 4 to pose 5
    if(0):
        #jointxRadList is rad
        cJ = [90.0000, -13.1531, -145.5463  ,       0,  158.6994  ,     0]
        nJ = [29.0546, -48.1043, -85.5551, -69.0162, 154.4306, 57.4913]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 5 to pose 6
    if(0):
        #jointxRadList is rad
        cJ = [29.0546, -48.1043, -85.5551, -69.0162, 154.4306, 57.4913]
        nJ = [35.8377, -52.5490, -75.0968, -66.1969, 152.3808, 76.4231]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 6 to pose 7
    if(0):
        #jointxRadList is rad
        cJ = [35.8377, -52.5490, -75.0968, -66.1969, 152.3808, 76.4231]
        nJ = [41.6335, -58.5910, -61.0694, -66.2566, 149.7381, 67.6951]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 7 to pose 8
    if(0):
        #jointxRadList is rad
        cJ = [41.6335, -58.5910, -61.0694, -66.2566, 149.7381, 67.6951]
        nJ = [38.6598, -64.6904, -47.0260, -68.5107, 159.5242, 62.9763]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()
           
    #from pose 8 to pose 9
    if(0):
        #jointxRadList is rad
        cJ = [38.6598, -64.6904, -47.0260, -68.5107, 159.5242, 62.9763]
        nJ = [33.0239, -57.7865, -62.9284, -76.6339, 142.0606, 39.6792]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 9 to pose 10
    if(0):
        #jointxRadList is rad
        cJ = [33.0239, -57.7865, -62.9284, -76.6339, 142.0606, 39.6792]
        nJ = [26.5651, -53.0130, -74.0132, -78.2435, 144.0816, 49.8591]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 10 to pose 11
    if(0):
        #jointxRadList is rad
        cJ = [26.5651, -53.0130, -74.0132, -78.2435, 144.0816, 49.8591]
        nJ = [24.4440, -58.8873, -60.3852, -77.4693, 153.8407, 43.3780]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 11 to pose 12
    if(0):
        #jointxRadList is rad
        cJ = [24.4440, -58.8873, -60.3852, -77.4693, 153.8407, 43.3780]
        nJ = [30.5792, -64.4254, -47.6348, -77.4869, 151.8693, 34.7930]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 12 to pose 13
    if(0):
        #jointxRadList is rad
        cJ = [30.5792, -64.4254, -47.6348, -77.4869, 151.8693, 34.7930]
        nJ = [36.0274, -73.9375, -25.7871, -82.9965, 149.5691, 63.0357]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 13 to pose 14
    if(0):
        #jointxRadList is rad
        cJ = [36.0274, -73.9375, -25.7871, -82.9965, 149.5691, 63.0357]
        nJ = [36.0274, -71.9836, -20.6368, -88.0956, 149.0162, 71.7611]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 14 to pose 15
    if(0):
        #jointxRadList is rad
        cJ = [36.0274, -71.9836, -20.6368, -88.0956, 149.0162, 71.7611]
        nJ = [30.5792, -61.1899, -44.8244, -80.7412, 150.7260, 41.5627]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 15 to pose 16
    if(0):
        #jointxRadList is rad
        cJ = [30.5792, -61.1899, -44.8244, -80.7412, 150.7260, 41.5627]
        nJ = [24.4440, -55.1994, -58.0227, -79.8390, 152.6491, 49.7654]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 16 to pose 17
    if(0):
        #jointxRadList is rad
        cJ = [24.4440, -55.1994, -58.0227, -79.8390, 152.6491, 49.7654]
        nJ = [26.5651, -48.8364, -71.8905, -80.6690, 142.3916, 56.3939]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 17 to pose 18
    if(0):
        #jointxRadList is rad
        cJ = [26.5651, -48.8364, -71.8905, -80.6690, 142.3916, 56.3939]
        nJ = [33.0239, -54.0106, -60.6252, -69.8396, 160.3051, 76.3233]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 18 to pose 19
    if(0):
        #jointxRadList is rad
        cJ = [33.0239, -54.0106, -60.6252, -69.8396, 160.3051, 76.3233]
        nJ = [38.6598, -61.4781, -44.1858, -72.8117, 158.0229, 70.2470]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 19 to pose 20
    if(0):
        #jointxRadList is rad
        cJ = [38.6598, -61.4781, -44.1858, -72.8117, 158.0229, 70.2470]
        nJ = [41.6335, -54.8794, -58.7236, -70.4089, 147.4984, 74.6884]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 20 to pose 21
    if(0):
        #jointxRadList is rad
        cJ = [41.6335, -54.8794, -58.7236, -70.4089, 147.4984, 74.6884]
        nJ = [35.8377, -48.3308, -72.9861, -69.4243, 149.9876, 83.1118]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 21 to pose 22
    if(0):
        #jointxRadList is rad
        cJ = [35.8377, -48.3308, -72.9861, -69.4243, 149.9876, 83.1118]
        nJ = [29.0546, -43.4484, -83.5165, -71.5271, 152.1680, 64.2736]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 22 to pose 23
    if(0):
        #jointxRadList is rad
        cJ = [29.0546, -43.4484, -83.5165, -71.5271, 152.1680, 64.2736]
        nJ = [0.0000, 0.0000, -83.5165, -71.5271, 152.1680, 0.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 23 to pose 24
    if(0):
        #jointxRadList is rad
        cJ = [0.0000, 0.0000, -83.5165, -71.5271, 152.1680, 0.0000]
        nJ = [0.0000, 0.0000, 0.0000, 0.0000, 152.1680, 0.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 24 to pose 25
    if(0):
        #jointxRadList is rad
        cJ = [0.0000, 0.0000, 0.0000, 0.0000, 152.1680, 0.0000]
        nJ = [0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()
    # ---------------------------end----------------------------- #

    #from pose 25 to pose 26
    if(0):
        #jointxRadList is rad
        cJ = [22.8337 , -43.5183 , -83.3663 , -75.8173 , 161.9175 ,  50.8473]
        nJ = [27.7585,  -46.0482 , -77.9204  ,-73.6128 , 157.2778  , 52.7171]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 26 to pose 27
    if(0):
        #jointxRadList is rad
        cJ = [27.7585,  -46.0482 , -77.9204  ,-73.6128 , 157.2778  , 52.7171]
        nJ = [32.2756,  -49.1992 , -71.1038 , -72.3240 , 152.5462  , 55.3467]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 27 to pose 28
    if(0):
        #jointxRadList is rad
        cJ = [32.2756,  -49.1992 , -71.1038 , -72.3240 , 152.5462  , 55.3467]
        nJ = [36.3844,  -53.0794 , -62.6599,  -72.2557 , 147.7007 ,  59.0844]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 28 to pose 29
    if(0):
        #jointxRadList is rad
        cJ = [36.3844,  -53.0794 , -62.6599,  -72.2557 , 147.7007 ,  59.0844]
        nJ = [40.1009 , -57.9365 , -52.0102  ,-73.9717 , 142.7356   ,64.6177]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 29 to pose 30
    if(0):
        #jointxRadList is rad
        cJ = [40.1009 , -57.9365 , -52.0102  ,-73.9717 , 142.7356   ,64.6177]
        nJ = [36.0274  ,-71.9836 , -20.6368,  -88.0956 , 159.0162 , 71.7611]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 30 to pose 31
    if(0):
        #jointxRadList is rad
        cJ = [36.0274  ,-71.9836 , -20.6368,  -88.0956 , 159.0162 , 71.7611]
        nJ = [32.4712 , -63.9120 , -38.7776 , -82.0423,  158.4151  ,45.0564]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 31 to pose 32
    if(0):
        #jointxRadList is rad
        cJ = [32.4712 , -63.9120 , -38.7776 , -82.0423,  158.4151  ,45.0564]
        nJ = [28.6105  ,-58.8954  ,-49.8968  ,-80.0342  ,153.0427  , 68.8135]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 32 to pose 33
    if(0):
        #jointxRadList is rad
        cJ = [28.6105  ,-58.8954  ,-49.8968  ,-80.0342  ,153.0427  , 68.8135]
        nJ = [24.4440 , -55.1994 , -58.0227 , -79.8390 , 142.6491 ,  49.7654]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 33 to pose 34
    if(0):
        #jointxRadList is rad
        cJ = [24.4440 , -55.1994 , -58.0227 , -79.8390 , 142.6491 ,  49.7654]
        nJ = [19.9831,  -52.3912 , -64.1617  ,-80.7671  ,152.1999 ,  31.9983]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 34 to pose 35
    if(0):
        #jointxRadList is rad
        cJ = [19.9831,  -52.3912 , -64.1617  ,-80.7671  ,152.1999 ,  31.9983]
        nJ = [0     ,    0 , -64.1617 , -80.7671 , 152.1999  ,       0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 35 to pose 36
    if(0):
        #jointxRadList is rad
        cJ = [ 0     ,    0 , -64.1617 , -80.7671 , 152.1999  ,       0]
        nJ = [ 0        , 0      ,   0     ,    0 , 152.1999    ,     0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 36 to pose 0
    if(0):
        #jointxRadList is rad
        cJ = [ 0        , 0      ,   0     ,    0 , 152.1999    ,     0]
        nJ = [ 0 ,        0     ,    0      ,   0  , 0     ,    0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

# 手眼标定点位2
def hand_eye2(pub):
    # 需要延时更长时间 令pause_more=1
    pause_more = 0

    # 如果需要自动运行令auto_run=1，否者只运行指定的pose
    auto_run = 0 # 默认不运行
    # 需要运行第几步就给 pose_step赋几
    pose_step = 0 # 默认不运行


    jointTarget = ti5_targetPos()

    jointTarget.numOfCmd = 7
    jointTarget.jointIDList = [1, 2, 3 ,4 ,5 ,6, 7]

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start+++*********")
    pp =0

    #from pose 1 to pose 2
    if(auto_run or pose_step==1):
        #jointxRadList is rad
        cJ = [0,0,0,0,0,0]
        nJ = [0,0,0,0,158.6994,0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()
        
    #from pose 2 to pose 3
    if(auto_run or pose_step==2):
        #jointxRadList is rad
        cJ = [0,0,0,0,158.6994,0]
        nJ = [ 0 ,     0 ,-145.5463  ,       0 , 158.6994  ,       0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 3 to pose 4
    if(auto_run or pose_step==3):
        #jointxRadList is rad
        cJ = [0 ,     0 ,-145.5463  ,       0 , 158.6994  ,       0]
        nJ = [90.0000, -13.1531, -145.5463  ,       0,  158.6994  ,     0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 4 to pose 5
    if(auto_run or pose_step==4):
        #jointxRadList is rad
        cJ = [90.0000, -13.1531, -145.5463  ,       0,  158.6994  ,     0]
        nJ = [40.0000, -76.5300, -14.1300, -88.2700, 180.0000, 135.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 5 to pose 6
    if(auto_run or pose_step==5):
        #jointxRadList is rad
        cJ = [40.0000, -76.5300, -14.1300, -88.2700, 180.0000, 135.0000]
        nJ = [40.0000, -76.5300, -14.1300, -88.2700, 165.0000, 120.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 6 to pose 7
    if(auto_run or pose_step==6):
        #jointxRadList is rad
        cJ = [40.0000, -76.5300, -14.1300, -88.2700, 165.0000, 120.0000]
        nJ = [40.0000, -76.5300, -14.1300, -88.2700, 150.0000, 105.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 7 to pose 8
    if(auto_run or pose_step==7):
        #jointxRadList is rad
        cJ = [40.0000, -76.5300, -14.1300, -88.2700, 150.0000, 105.0000]
        nJ = [40.0000, -76.5300, -14.1300, -88.2700, 135.0000, 90.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()
           
    #from pose 8 to pose 9
    if(auto_run or pose_step==8):
        #jointxRadList is rad
        cJ = [40.0000, -76.5300, -14.1300, -88.2700, 135.0000, 90.0000]
        nJ = [50.0000, -76.5300, -14.1300, -88.2700, 120.0000, 75.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 9 to pose 10
    if(auto_run or pose_step==9):
        #jointxRadList is rad
        cJ = [50.0000, -76.5300, -14.1300, -88.2700, 120.0000, 75.0000]
        nJ = [50.0000, -76.5300, -14.1300, -88.2700, 105.0000, 60.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 10 to pose 11
    if(auto_run or pose_step==10):
        #jointxRadList is rad
        cJ = [50.0000, -76.5300, -14.1300, -88.2700, 105.0000, 60.0000]
        nJ = [50.0000, -76.5300, -14.1300, -88.2700, 90.0000, 45.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 11 to pose 12
    if(auto_run or pose_step==11):
        #jointxRadList is rad
        cJ = [50.0000, -76.5300, -14.1300, -88.2700, 90.0000, 45.0000]
        nJ = [50.0000, -76.5300, -14.1300, -88.2700, 75.0000, 135.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 12 to pose 13
    if(auto_run or pose_step==12):
        #jointxRadList is rad
        cJ = [50.0000, -76.5300, -14.1300, -88.2700, 75.0000, 135.0000]
        nJ = [50.0000, -76.5300, -14.1300, -88.2700, 60.0000, 105.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 13 to pose 14
    if(auto_run or pose_step==13):
        #jointxRadList is rad
        cJ = [50.0000, -76.5300, -14.1300, -88.2700, 60.0000, 105.0000]
        nJ = [50.0000, -76.5300, -14.1300, -88.2700, 45.0000, 75.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 14 to pose 15
    if(auto_run or pose_step==14):
        #jointxRadList is rad
        cJ = [50.0000, -76.5300, -14.1300, -88.2700, 45.0000, 75.0000]
        nJ = [40.0000, -76.5300, -14.1300, -88.2700, 60.0000, 60.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 15 to pose 16
    if(auto_run or pose_step==15):
        #jointxRadList is rad
        cJ = [40.0000, -76.5300, -14.1300, -88.2700, 60.0000, 60.0000]
        nJ = [40.0000, -76.5300, -14.1300, -88.2700, 75.0000, 75.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 16 to pose 17
    if(auto_run or pose_step==16):
        #jointxRadList is rad
        cJ = [40.0000, -76.5300, -14.1300, -88.2700, 75.0000, 75.0000]
        nJ = [40.0000, -76.5300, -14.1300, -88.2700, 90.0000, 90.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 17 to pose 18
    if(auto_run or pose_step==17):
        #jointxRadList is rad
        cJ = [40.0000, -76.5300, -14.1300, -88.2700, 90.0000, 90.0000]
        nJ = [40.0000, -76.5300, -14.1300, -88.2700, 105.0000, 105.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 18 to pose 19
    if(auto_run or pose_step==18):
        #jointxRadList is rad
        cJ = [40.0000, -76.5300, -14.1300, -88.2700, 105.0000, 105.0000]
        nJ = [40.0000, -76.5300, -14.1300, -88.2700, 120.0000, 120.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 19 to pose 20
    if(auto_run or pose_step==19):
        #jointxRadList is rad
        cJ = [40.0000, -76.5300, -14.1300, -88.2700, 120.0000, 120.0000]
        nJ = [40.0000, -76.5300, -14.1300, -88.2700, 135.0000, 135.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 20 to pose 21
    if(auto_run or pose_step==20):
        #jointxRadList is rad
        cJ = [40.0000, -76.5300, -14.1300, -88.2700, 135.0000, 135.0000]
        nJ = [45.0000, -65.0000, -10.0000, -88.2700, 120.0000, 120.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 21 to pose 22
    if(auto_run or pose_step==21):
        #jointxRadList is rad
        cJ = [45.0000, -65.0000, -10.0000, -88.2700, 120.0000, 120.0000]
        nJ = [40.0000, -65.0000, -50.0000, -88.2700, 115.0000, 90.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 22 to pose 23
    if(auto_run or pose_step==22):
        #jointxRadList is rad
        cJ = [40.0000, -65.0000, -50.0000, -88.2700, 115.0000, 90.0000]
        nJ = [0.0000, 0.0000, -50.0000, -88.2700, 115.0000, 0.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 23 to pose 24
    if(auto_run or pose_step==23):
        #jointxRadList is rad
        cJ = [0.0000, 0.0000, -50.0000, -88.2700, 115.0000, 0.0000]
        nJ = [0.0000, 0.0000, 0.0000, 0.0000, 115.0000, 0.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 24 to pose 25
    if(auto_run or pose_step==24):
        #jointxRadList is rad
        cJ = [0.0000, 0.0000, 0.0000, 0.0000, 115.0000, 0.0000]
        nJ = [0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()
    # ---------------------------end----------------------------- #

    #from pose 25 to pose 26
    if(0):
        #jointxRadList is rad
        cJ = [22.8337 , -43.5183 , -83.3663 , -75.8173 , 161.9175 ,  50.8473]
        nJ = [27.7585,  -46.0482 , -77.9204  ,-73.6128 , 157.2778  , 52.7171]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 26 to pose 27
    if(0):
        #jointxRadList is rad
        cJ = [27.7585,  -46.0482 , -77.9204  ,-73.6128 , 157.2778  , 52.7171]
        nJ = [32.2756,  -49.1992 , -71.1038 , -72.3240 , 152.5462  , 55.3467]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 27 to pose 28
    if(0):
        #jointxRadList is rad
        cJ = [32.2756,  -49.1992 , -71.1038 , -72.3240 , 152.5462  , 55.3467]
        nJ = [36.3844,  -53.0794 , -62.6599,  -72.2557 , 147.7007 ,  59.0844]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 28 to pose 29
    if(0):
        #jointxRadList is rad
        cJ = [36.3844,  -53.0794 , -62.6599,  -72.2557 , 147.7007 ,  59.0844]
        nJ = [40.1009 , -57.9365 , -52.0102  ,-73.9717 , 142.7356   ,64.6177]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 29 to pose 30
    if(0):
        #jointxRadList is rad
        cJ = [40.1009 , -57.9365 , -52.0102  ,-73.9717 , 142.7356   ,64.6177]
        nJ = [36.0274  ,-71.9836 , -20.6368,  -88.0956 , 159.0162 , 71.7611]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 30 to pose 31
    if(0):
        #jointxRadList is rad
        cJ = [36.0274  ,-71.9836 , -20.6368,  -88.0956 , 159.0162 , 71.7611]
        nJ = [32.4712 , -63.9120 , -38.7776 , -82.0423,  158.4151  ,45.0564]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 31 to pose 32
    if(0):
        #jointxRadList is rad
        cJ = [32.4712 , -63.9120 , -38.7776 , -82.0423,  158.4151  ,45.0564]
        nJ = [28.6105  ,-58.8954  ,-49.8968  ,-80.0342  ,153.0427  , 68.8135]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 32 to pose 33
    if(0):
        #jointxRadList is rad
        cJ = [28.6105  ,-58.8954  ,-49.8968  ,-80.0342  ,153.0427  , 68.8135]
        nJ = [24.4440 , -55.1994 , -58.0227 , -79.8390 , 142.6491 ,  49.7654]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 33 to pose 34
    if(0):
        #jointxRadList is rad
        cJ = [24.4440 , -55.1994 , -58.0227 , -79.8390 , 142.6491 ,  49.7654]
        nJ = [19.9831,  -52.3912 , -64.1617  ,-80.7671  ,152.1999 ,  31.9983]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 34 to pose 35
    if(0):
        #jointxRadList is rad
        cJ = [19.9831,  -52.3912 , -64.1617  ,-80.7671  ,152.1999 ,  31.9983]
        nJ = [0     ,    0 , -64.1617 , -80.7671 , 152.1999  ,       0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 35 to pose 36
    if(0):
        #jointxRadList is rad
        cJ = [ 0     ,    0 , -64.1617 , -80.7671 , 152.1999  ,       0]
        nJ = [ 0        , 0      ,   0     ,    0 , 152.1999    ,     0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 36 to pose 0
    if(0):
        #jointxRadList is rad
        cJ = [ 0        , 0      ,   0     ,    0 , 152.1999    ,     0]
        nJ = [ 0 ,        0     ,    0      ,   0  , 0     ,    0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

# 循环抓取物体 
def grab3target(pub):
    # 需要延时更长时间 令pause_more=1
    pause_more = 0

    # 如果需要自动运行令auto_run=1，否者只运行指定的pose
    auto_run = 1 # 默认不运行
    # 需要运行第几步就给 pose_step赋几
    pose_step = 0 # 默认不运行

    jointTarget = ti5_targetPos()

    jointTarget.numOfCmd = 7
    jointTarget.jointIDList = [1, 2, 3 ,4 ,5 ,6, 7]

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start+++*********")
    pp =0

    #from pose 1 to pose 2
    if(auto_run or pose_step==1):
        #jointxRadList is rad
        cJ = [0,0,0,0,0,0]
        nJ = [0,0,-90,0,0,0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()


    # ++++++++from point1 grab cube1 and put it to point2+++++++++++++++++++
    print("from point1 grab cube1 and put it to point2")
    #from pose 2 to pose 3(point1_up)
    if(auto_run or pose_step==2):
        #jointxRadList is rad
        cJ = [0,0,-90,0,0,0]
        nJ = [-30.96,  -50.38,  -16.07,  0,  -23.56,  -120.96]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 3 to pose 4(point1_down) and grab  
    if(auto_run or pose_step==3):
        #jointxRadList is rad
        cJ = [-30.96,  -50.38,  -16.07,  0,  -23.56,  -120.96]
        nJ = [-30.96,  -46.01,  -54.13,  0,  10.14,  -120.96]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 4 to pose 5(3(point1_up)) 
    if(auto_run or pose_step==4):
        #jointxRadList is rad
        cJ = [-30.96,  -46.01,  -54.13,  0,  10.14,  -120.96]
        nJ = [-30.96,  -50.38,  -16.07,  0,  -23.56,  -120.96]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 5 to pose 6(point2_up) 
    if(auto_run or pose_step==5):
        #jointxRadList is rad
        cJ = [-30.96,  -50.38,  -16.07,  0,  -23.56,  -120.96]
        nJ = [30.96,  -50.38,  -16.07,  0,  -23.56,  -59.04]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 6 to pose 7(point2_down) and lose
    if(auto_run or pose_step==6):
        #jointxRadList is rad
        cJ = [30.96,  -50.38,  -16.07,  0,  -23.56,  -59.04]
        nJ = [30.96,  -46.01,  -54.13,  0,  10.14,  -59.04]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 7 to pose 8(6(point2_up)) 
    if(auto_run or pose_step==7):
        #jointxRadList is rad
        cJ = [30.96,  -46.01,  -54.13,  0,  10.14,  -59.04]
        nJ = [30.96,  -50.38,  -16.07,  0,  -23.56,  -59.04]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    # ++++++++from point3 grab cube2 and put it to point1+++++++++++++++++++
    print("from point3 grab cube2 and put it to point1")

    #from pose 8 to pose 9(point3_up) 
    if(auto_run or pose_step==8):
        #jointxRadList is rad
        cJ = [30.96,  -46.01,  -54.13,  0,  10.14,  -59.04]
        nJ = [0,  -30.24,  -54.13,  0,  -5.63,  -90]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 9 to pose 10(point3_down) and grab
    if(auto_run or pose_step==9):
        #jointxRadList is rad
        cJ = [0,  -30.24,  -54.13,  0,  -5.63,  -90]
        nJ = [0,  -33.44,  -77.82,  0,  21.26,  -90]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 10 to pose 11(9(point3_up)) 
    if(auto_run or pose_step==10):
        #jointxRadList is rad
        cJ = [0,  -33.44,  -77.82,  0,  21.26,  -90]
        nJ = [0,  -30.24,  -54.13,  0,  -5.63,  -90]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()  

    #from pose 11 to pose 12(3(point1_up))
    if(auto_run or pose_step==11):
        #jointxRadList is rad
        cJ = [0,  -30.24,  -54.13,  0,  -5.63,  -90]
        nJ = [-30.96,  -50.38,  -16.07,  0,  -23.56,  -120.96]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()                    

    #from pose 12 to pose 13(4(point1_down)) and lose
    if(auto_run or pose_step==12):
        #jointxRadList is rad
        cJ = [-30.96,  -50.38,  -16.07,  0,  -23.56,  -120.96]
        nJ = [-30.96,  -46.01,  -54.13,  0,  10.14,  -120.96]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep() 

    #from pose 13 to pose 14(3(point1_up)) 
    if(auto_run or pose_step==13):
        #jointxRadList is rad
        cJ = [-30.96,  -46.01,  -54.13,  0,  10.14,  -120.96]
        nJ = [-30.96,  -50.38,  -16.07,  0,  -23.56,  -120.96]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep() 

    # ++++++++from point2 grab cube1 and put it to point3+++++++++++++++++++
    print("from point2 grab cube1 and put it to point3")
    #from pose 14 to pose 15(point2_up) 
    if(auto_run or pose_step==14):
        #jointxRadList is rad
        cJ = [-30.96,  -50.38,  -16.07,  0,  -23.56,  -120.96]
        nJ = [30.96,  -50.38,  -16.07,  0,  -23.56,  -59.04]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 15 to pose 16(point2_down) and grab
    if(auto_run or pose_step==15):
        #jointxRadList is rad
        cJ = [30.96,  -50.38,  -16.07,  0,  -23.56,  -59.04]
        nJ = [30.96,  -46.01,  -54.13,  0,  10.14,  -59.04]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 16 to pose 17(6(point2_up)) 
    if(auto_run or pose_step==16):
        #jointxRadList is rad
        cJ = [30.96,  -46.01,  -54.13,  0,  10.14,  -59.04]
        nJ = [30.96,  -50.38,  -16.07,  0,  -23.56,  -59.04]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 17 to pose 18(point3_up) 
    if(auto_run or pose_step==17):
        #jointxRadList is rad
        cJ = [30.96,  -46.01,  -54.13,  0,  10.14,  -59.04]
        nJ = [0,  -30.24,  -54.13,  0,  -5.63,  -90]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 18 to pose 19(point3_down) and lose
    if(auto_run or pose_step==18):
        #jointxRadList is rad
        cJ = [0,  -30.24,  -54.13,  0,  -5.63,  -90]
        nJ = [0,  -33.44,  -77.82,  0,  21.26,  -90]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 19 to pose 20(9(point3_up)) 
    if(auto_run or pose_step==19):
        #jointxRadList is rad
        cJ = [0,  -33.44,  -77.82,  0,  21.26,  -90]
        nJ = [0,  -30.24,  -54.13,  0,  -5.63,  -90]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #+++++++++back to pose 2 and start next loop++++++++++++++
    print("back to pose 2 and start next loop")
    #from pose 20 to pose 2
    if(auto_run or pose_step==20):
        #jointxRadList is rad
        cJ = [0,  -30.24,  -54.13,  0,  -5.63,  -90]
        nJ = [0,0,-90,0,0,0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()



    pass


def testMotor(pub):
    jointTarget = ti5_targetPos()

    jointTarget.numOfCmd = 7
    jointTarget.jointIDList = [1, 2, 3 ,4 ,5 ,6, 7]

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start+++*********")
    pp =0
    while 1:
        # from pose 0 to pose 1
        if(1):
            #jointxRadList is rad
            cJ = [0,0,0,0,0,0]
            nJ = [0,0,-1.5708,0,0,0.5]
            jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)

            jointTarget.joint7RadList = [0,     0]
            jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

            pp =pp+1
            print("*******pose:",pp)
            pub.publish(jointTarget)
            print("ok")
            rate.sleep()
            rate.sleep()
            rate.sleep()
            rate.sleep()

        # from pose 1 to pose 0
        if(1):
            #jointxRadList is rad
            cJ = [0,0,-1.5708,0,0,0.5]
            nJ = [0,0,0,0,0,0]
            jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)

            jointTarget.joint7RadList = [0,     0]
            jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

            pp =pp+1
            print("*******pose:",pp)
            pub.publish(jointTarget)
            print("ok")
            rate.sleep()
            rate.sleep()
            rate.sleep()
            rate.sleep()


def testError(pub):
    jointTarget = ti5_targetPos()

    jointTarget.numOfCmd = 7
    jointTarget.jointIDList = [1, 2, 3 ,4 ,5 ,6, 7]

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start+++*********")
    pp =0

    # test 1
    if(1):
        #jointxRadList is rad
        cJ = [0,0,0,0,0,0]
        nJ = [90,0,0,0,0,0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)

        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()

    # test 2
    if(1):
        #jointxRadList is rad
        cJ = [90,0,0,0,0,0]
        nJ = [0,0,0,0,0,0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)

        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()



# 按照给定的关节角 Jdeg(最小为6个关节*2组) 控制机械臂
def moveJ(pub,Jdeg):
    # 需要延时更长时间 令pause_more=1
    pause_more = 0

    # 如果需要自动运行令auto_run=1，否者只运行指定的pose
    auto_run = 0 # 默认不运行
    # 需要运行第几步就给 pose_step赋几
    pose_step = 23 # 默认不运行


    jointTarget = ti5_targetPos()

    jointTarget.numOfCmd = 7
    jointTarget.jointIDList = [1, 2, 3 ,4 ,5 ,6, 7]

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system

    # 关节角组数   步数为 groups_Jdeg-1
    groups_Jdeg = len(Jdeg)/6

    print("*********+++++start+++*********")
    pp =0

    # 自动按顺序移动
    if (auto_run):
        for i in range(0,groups_Jdeg-1):
            #jointxRadList is rad
            cJ = [  Jdeg[0+(i*6)],\
                    Jdeg[1+(i*6)],\
                    Jdeg[2+(i*6)],\
                    Jdeg[3+(i*6)],\
                    Jdeg[4+(i*6)],\
                    Jdeg[5+(i*6)]]
            
            nJ = [  Jdeg[0+((i+1)*6)],\
                    Jdeg[1+((i+1)*6)],\
                    Jdeg[2+((i+1)*6)],\
                    Jdeg[3+((i+1)*6)],\
                    Jdeg[4+((i+1)*6)],\
                    Jdeg[5+((i+1)*6)]]
            print("cJ",i+1,cJ[0],cJ[1],cJ[2],cJ[3],cJ[4],cJ[5])
            print("nJ",i+1+1,nJ[0],nJ[1],nJ[2],nJ[3],nJ[4],nJ[5])

            jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
        
            jointTarget.joint7RadList = [0,     0]
            jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

            pp =pp+1
            print("*******pose:",pp)
            pub.publish(jointTarget)
            print("ok")
            rate.sleep()
            rate.sleep()
            rate.sleep()
            if pause_more==1:
                rate.sleep()
                rate.sleep()
                rate.sleep()        
        pass


    # 逐步移动
    if((not auto_run) and (pose_step>=1) and (pose_step<=groups_Jdeg-1)):
        #jointxRadList is rad
        # pose_step是从1开始的 需要减1
        cJ = [  Jdeg[0+((pose_step-1)*6)],\
                Jdeg[1+((pose_step-1)*6)],\
                Jdeg[2+((pose_step-1)*6)],\
                Jdeg[3+((pose_step-1)*6)],\
                Jdeg[4+((pose_step-1)*6)],\
                Jdeg[5+((pose_step-1)*6)]]
        
        nJ = [  Jdeg[0+(pose_step*6)],\
                Jdeg[1+(pose_step*6)],\
                Jdeg[2+(pose_step*6)],\
                Jdeg[3+(pose_step*6)],\
                Jdeg[4+(pose_step*6)],\
                Jdeg[5+(pose_step*6)]]
         
        print("cJ",pose_step,cJ[0],cJ[1],cJ[2],cJ[3],cJ[4],cJ[5])
        print("nJ",pose_step+1,nJ[0],nJ[1],nJ[2],nJ[3],nJ[4],nJ[5])

        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
    
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        print("*******pose:",pose_step)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()        
        pass


def grab_1(pub):
    # 需要延时更长时间 令pause_more=1
    pause_more = 0

    # 如果需要自动运行令auto_run=1，否者只运行指定的pose
    auto_run = 0 # 默认不运行
    # 需要运行第几步就给 pose_step赋几
    pose_step = 0 # 默认不运行

    jointTarget = ti5_targetPos()

    jointTarget.numOfCmd = 7
    jointTarget.jointIDList = [1, 2, 3 ,4 ,5 ,6, 7]

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start+++*********")
    pp =0

    #from pose 1 to pose 2
    if(auto_run or pose_step==1):
        #jointxRadList is rad
        cJ = [0, 0, 0, 0, 0, 0]
        nJ = [0, 0, 0, 0, 0, 0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 2 to pose 3
    if(auto_run or pose_step==2):
        #jointxRadList is rad
        cJ = [0,0,0,0,0,0]
        nJ = [0,0,0,0,0,0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    # #from pose 3 to pose 4
    # if(auto_run or pose_step==3):
    #     #jointxRadList is rad
    #     cJ = [32.222,-36.787,-38.654,0,-14.558,-57.777]
    #     nJ = [32.222,-41.409,-76.408,0,27.818,-57.777]
    #     jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
    #     jointTarget.joint7RadList = [-400000,     -400000]
    #     jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

    #     pp =pp+1
    #     print("*******pose:",pp)
    #     pub.publish(jointTarget)
    #     print("ok")
    #     rate.sleep()
    #     rate.sleep()
    #     rate.sleep()

    #     if pause_more==1:
    #         rate.sleep()
    #         rate.sleep()

    # #from pose 4 to pose 5(3)
    # if(auto_run or pose_step==4):
    #     #jointxRadList is rad
    #     cJ = [32.222,-41.409,-76.408,0,27.818,-57.777]
    #     nJ = [32.222,-36.787,-38.654,0,-14.558,-57.777]
    #     jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
    #     jointTarget.joint7RadList = [-400000,     -400000]
    #     jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

    #     pp =pp+1
    #     print("*******pose:",pp)
    #     pub.publish(jointTarget)
    #     print("ok")
    #     rate.sleep()
    #     rate.sleep()
    #     rate.sleep()
    #     if pause_more==1:
    #         rate.sleep()
    #         rate.sleep()

    # #from pose 5 to pose 6
    # if(auto_run or pose_step==5):
    #     #jointxRadList is rad
    #     cJ = [32.222,-36.787,-38.654,0,-14.558,-57.777]
    #     nJ = [0,0,-90,0,0,0]
    #     jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
    #     jointTarget.joint7RadList = [-400000,     -400000]
    #     jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

    #     pp =pp+1
    #     print("*******pose:",pp)
    #     pub.publish(jointTarget)
    #     print("ok")
    #     rate.sleep()
    #     rate.sleep()
    #     rate.sleep()
    #     if pause_more==1:
    #         rate.sleep()
    #         rate.sleep()



    pass

# 将机械臂折叠，注意防撞！！！
def fold_arm(pub):
    # 需要延时更长时间 令pause_more=1
    pause_more = 1

    # 如果需要自动运行令auto_run=1，否者只运行指定的pose
    auto_run = 0 # 默认不运行
    # 需要运行第几步就给 pose_step赋几
    pose_step = 0 # 默认不运行

    jointTarget = ti5_targetPos()

    jointTarget.numOfCmd = 7
    jointTarget.jointIDList = [1, 2, 3 ,4 ,5 ,6, 7]

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system
    print("*********+++++start+++*********")
    pp =0

    #from pose 1 to pose 2
    if(auto_run or pose_step==1):
        #jointxRadList is rad
        cJ = [0,0,0,0,0,0]
        nJ = [0,0,0,0,220,0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,     0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()

    #from pose 2 to pose 3
    if(auto_run or pose_step==2):
        #jointxRadList is rad
        cJ = [0,0,0,0,220,0]
        nJ = [0,0,-150,0,220,0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,  0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()
    
        #from pose 3 to pose 4
    if(auto_run or pose_step==3):
        #jointxRadList is rad
        cJ = [0,0,-150,0,220,0]
        nJ = [0,90,-150,0,220,0]
        jointTarget = do_hand_eyeJList(jointTarget,cJ,nJ)
      
        jointTarget.joint7RadList = [0,    0]
        jointTarget.cmdList = [30,         30,      30,       30,       30,     30,   30]

        pp =pp+1
        print("*******pose:",pp)
        pub.publish(jointTarget)
        print("ok")
        rate.sleep()
        rate.sleep()
        rate.sleep()
        if pause_more==1:
            rate.sleep()
            rate.sleep()
    
    pass


##############################################################################

if  __name__== "__main__":
    rospy.init_node("jointTargetTest_pub_node")
    pub = rospy.Publisher("my_jointTargetTest",ti5_targetPos,queue_size=1000)
    
    # fold_arm(pub)

    # grab_1(pub)

    # grab3target(pub)

    # testMotor(pub)

    # hand_eye(pub)

    # hand_eye2(pub)

    # testError(pub)

    # j_deg = getJdeg("/home/wheeltec/Desktop/hJ.txt")

    # for i in range(0,25):
    #     print("j_deg[",i+1,"]:",j_deg[0+6*i], j_deg[1+6*i], j_deg[2+6*i], j_deg[3+6*i], j_deg[4+6*i], j_deg[5+6*i])
    #   #  print(f"j_deg[{i+1}]:\n{j_deg[0+6*i]}, {j_deg[1+6*i]}, {j_deg[2+6*i]}, {j_deg[3+6*i]}, {j_deg[4+6*i]}, {j_deg[5+6*i]}\n")
    #     pass

    # moveJ(pub,j_deg)
